#include"gpio.h"
#include"RTX51TNY.H"
#include"LED.h" 
#include"KEY.h"
#include"Voltage.h"
#include"UART.h"
#include"nvic.h"
#include"switch.h"
#include"Buzzer.h"
#include"Ultrasonic.h"
#include"STC8H_PWM.h"
#include"Motor.h"
#include"Track.h"

#define TASK_MAIN 0
#define TASK_LED 1
#define TASK_KEY 2
#define TASK_VOLTAGE 3
#define TASK_BUZZER 4
#define TASK_ULTRASONIC 5
#define TASK_MOTOR 6
#define TASK_TRACK 7
#define TASK_BT 8

u16 hz[]={1047,1175,1319,1397,1568,1760,1976,2093};

void UART_config(void) {
	//串口1
    COMx_InitDefine		COMx_InitStructure;					//结构定义
    COMx_InitStructure.UART_Mode      = UART_8bit_BRTx;	//模式, UART_ShiftRight,UART_8bit_BRTx,UART_9bit,UART_9bit_BRTx
    COMx_InitStructure.UART_BRT_Use   = BRT_Timer1;			//选择波特率发生器, BRT_Timer1, BRT_Timer2 (注意: 串口2固定使用BRT_Timer2)
    COMx_InitStructure.UART_BaudRate  = 115200ul;			//波特率, 一般 110 ~ 115200
    COMx_InitStructure.UART_RxEnable  = ENABLE;				//接收允许,   ENABLE或DISABLE
    COMx_InitStructure.BaudRateDouble = DISABLE;			//波特率加倍, ENABLE或DISABLE
    UART_Configuration(UART1, &COMx_InitStructure);		//初始化串口1 UART1,UART2,UART3,UART4

  	NVIC_UART1_Init(ENABLE,Priority_1);		//中断使能, ENABLE/DISABLE; 优先级(低到高) Priority_0,Priority_1,Priority_2,Priority_3
    UART1_SW(UART1_SW_P30_P31);		// 引脚选择, UART1_SW_P30_P31,UART1_SW_P36_P37,UART1_SW_P16_P17,UART1_SW_P43_P44

	//串口2
    COMx_InitStructure.UART_Mode      = UART_8bit_BRTx;	//模式, UART_ShiftRight,UART_8bit_BRTx,UART_9bit,UART_9bit_BRTx
    COMx_InitStructure.UART_BRT_Use   = BRT_Timer2;			//选择波特率发生器, BRT_Timer1, BRT_Timer2 (注意: 串口2固定使用BRT_Timer2)
    COMx_InitStructure.UART_BaudRate  = 115200ul;			//波特率, 一般 110 ~ 115200
    COMx_InitStructure.UART_RxEnable  = ENABLE;				//接收允许,   ENABLE或DISABLE
    COMx_InitStructure.BaudRateDouble = DISABLE;			//波特率加倍, ENABLE或DISABLE
    UART_Configuration(UART2, &COMx_InitStructure);		//初始化串口1 UART1,UART2,UART3,UART4

  	NVIC_UART1_Init(ENABLE,Priority_1);		//中断使能, ENABLE/DISABLE; 优先级(低到高) Priority_0,Priority_1,Priority_2,Priority_3
    UART2_SW(UART2_SW_P10_P11);		// 引脚选择, UART1_SW_P30_P31,UART1_SW_P36_P37,UART1_SW_P16_P17,UART1_SW_P43_P44
}

void task_led() _task_ TASK_LED
{
    LED_Init();
    LED_Off(ALL);

    while(1)
    {
        //同时点亮
        LED_On(ALL);
        os_wait2(K_TMO,100);
        //同时熄灭
        LED_Off(ALL);
        os_wait2(K_TMO,100);
    }
}

// void Motor_start2()
// {
//     PWMx_Duty duty;
//     percent+=direction;

//     if(percent>=100)
//     {
//         direction=-10;
//     }
//     else if(percent<=0)
//     {
//         direction=10;
//     }
//     duty.PWM3_Duty=PERIOD*percent/100;
//     UpdatePwm(PWM3,&duty);
// }
void key_up()
{
    // LED_On(ALL);
}
u8 count=0;
void key_down()
{
    // LED_Off(ALL);
    // u8 i;
    // for(i=0;i<sizeof(hz)/sizeof(16);i++)
    // {
    //     Buzzer_Play(hz[i]);
    //     os_wait2(K_TMO,200);
    // }
    // Buzzer_Stop();

    // float distance=Ultrasonic_Get();
    // printf("distance=%.2f cm\n",distance);

    count++;
    switch (count)
    {
        case 1:
            Motor_Forward(30,MODE_MID);
            break;
        case 2:
            Motor_Forward(30,MODE_LEFT);
            break;
        case 3:
            Motor_Forward(30,MODE_RIGHT);
            break;
        case 4:
            Motor_Backward(30,MODE_MID);
            break;
        case 5:
            Motor_Backward(30,MODE_LEFT);
            break;
        case 6:
            Motor_Backward(30,MODE_RIGHT);
            break;
        case 7:
            Motor_Left(30);
            break;
        case 8:
            Motor_Right(30);
            break;
        case 9:
            Motor_Translate(30,MODE_LEFT);
            break;
        case 10:
            Motor_Translate(30,MODE_RIGHT);
            break;
        case 11:
            Motor_Around(30,MODE_LEFT);
            break;
        case 12:    
            Motor_Around(30,MODE_RIGHT);
            break;
        case 13:
            Motor_Stop();
            break;
        default:
            count=0;
            break;
    }
}
void task_key() _task_ TASK_KEY
{
    KEY_Init();
    while(1)
    {
        KEY_Scan(key_up,key_down);
        os_wait2(K_TMO,4);
    }
}

void task_voltage() _task_ TASK_VOLTAGE
{
    Voltage_Init();
    while(1)
    {
         float v=Voltage_Get();
         printf("voltage=%.2f",v); 
         os_wait2(K_TMO,200);
    }
}

void task_buzzer() _task_ TASK_BUZZER
{

}

void task_ultrasonic() _task_ TASK_ULTRASONIC
{

}

void task_motor() _task_ TASK_MOTOR 
{

}
//===================循迹===========================

void task_track() _task_ TASK_TRACK
{
    while(1)
    {//led1~5 从左到右
        int count=Track_Get();

        if(count<=-48)
        {//led1 或led1 led2 左大转
            Motor_Left(40);
        }
        else if(count<=-16)
        {//led2 或led2 led3 左小转
            Motor_Left(20);
        }
        else if(count>=48)
        {
            Motor_Right(40);
        }
        else if(count>=16)
        {
            Motor_Right(20);
        }
        else
        {//
            Motor_Forward(30,MODE_MID);
        }

        os_wait2(K_TMO,4);
    }
}

//========================蓝牙========================
void bt_config()
{
    GPIO_InitTypeDef	GPIO_InitStructure;		//结构定义
	GPIO_InitStructure.Pin  = GPIO_Pin_0|GPIO_Pin_1;//指定要初始化的IO,
	GPIO_InitStructure.Mode = GPIO_PullUp;	//指定IO的输入或输出方式,GPIO_PullUp,GPIO_HighZ,GPIO_OUT_OD,GPIO_OUT_PP
	GPIO_Inilize(GPIO_P1, &GPIO_InitStructure);//初始化
}

void handle_bt(u8 *dat)
{
    int x,y;

    if(dat[0]!=0xDD && dat[1]!=0x77)//如果帧头不对
    return;

    if(dat[4]==0x01)//按下A键
    {
        LED_On(LEFT);
    }
    if(dat[5]==0x01)
    {
        LED_On(RIGHT);
    }
    if(dat[6]==0x01)
    {
        LED_On(ALL);
    }
    if(dat[7]==0x01)
    {
        LED_Off(ALL);
    }

    if(dat[2]!=0 || dat[3]!=0)
    {//摇杆操控
        x=dat[2],y=dat[3];
        Motor_Move(dat[2],dat[3]);
    }
    else Motor_Stop();
}
void task_bt() _task_ TASK_BT
{
    bt_config();
    while(1)
    {
        u8 i;
        //1.读取串口1数据，如果有数据则用串口2发送出去给蓝牙
        if(COM1.RX_Cnt>0)
        {
            for(i=0;i<COM1.RX_Cnt;i++)
            TX2_write2buff(RX1_Buffer[i]);
            COM1.RX_Cnt=0;
        }
        //2.读取串口2数据，如果有数据用串口1发送给PC
        if(COM2.RX_Cnt>0)
        {
            for(i=0;i<COM2.RX_Cnt;i++)
            TX1_write2buff(RX2_Buffer[i]);

            for(i=0;i<COM2.RX_Cnt;i+=8)
            handle_bt(&RX2_Buffer[i]);//获取每个帧头地址，可以将每个帧都做处理

            COM2.RX_Cnt=0;
        }
        os_wait2(K_TMO,4);
    }
}

void task_main() _task_ TASK_MAIN
{
    EA=1;//串口中断
    LED_Init();
    UART_config();
    Buzzer_Init();
    Motor_Init();
    Ultrasonic_Init();

    os_create_task(TASK_KEY);

    os_delete_task(TASK_MAIN);
}